clc;clear;clearvars;
% 随机生成20个数据,P10个,Q10个
num_initial1 = 10;
num_initial2 = 10;
num_vari = 60;
% 搜索区间
upper_bound = 5.12;
lower_bound = -5.12;
% 迭代时计算y值总的个数
eval_num = 12000;
% K表示划分子空间的个数,sub_num表示每个子空间的维度
K = 6;
sub_num = num_vari / K;
% 算法的迭代次数
iter = eval_num / K;
w = 1;
% 随机生成10\10个数据,并获得其评估值
sample_x1 = lhsdesign(num_initial1, num_vari).*(upper_bound - lower_bound) + lower_bound.*ones(num_initial1, num_vari);
sample_y1 = Rastrigin(sample_x1);
sample_x2 = lhsdesign(num_initial2, num_vari).*(upper_bound - lower_bound) + lower_bound.*ones(num_initial2, num_vari);
sample_y2 = Rastrigin(sample_x2);
Fmin = zeros(iter, 1);
aver_Fmin = zeros(iter, 1);

for n = 1 : 100
    n1 = 1;
    % 初始化一些参数
    pbestx1 = sample_x1;
    pbesty1 = sample_y1;
    pbestxx = sample_x2;
    pbestyy = sample_y2;
    % 当前位置信息presentx
    presentx1 = lhsdesign(num_initial1, num_vari).*(upper_bound - lower_bound) + lower_bound.*ones(num_initial1, num_vari);
    presentxx = lhsdesign(num_initial2, num_vari).*(upper_bound - lower_bound) + lower_bound.*ones(num_initial2, num_vari);
    vx1 = sample_x1;
    vxx = sample_x2;
    [fmin1, gbest1] = min(pbesty1);
    [fmin2, gbest2] = min(pbestyy);
    global_best_x1 = pbestx1(gbest1, :);
    fprintf("n: %.4f\n", n);
    % fprintf("iter 0 fmin: %.4f\n", fmin);
    for i = 1 : iter
        for i1 = 1 : K
            ind = ((1 + (i1 - 1) * sub_num) : i1 * sub_num);
            r1 = rand(num_initial1, sub_num);
            % cpso更新下一步的位置,这里可以设置一下超过搜索范围的就设置为边界
            vx1(:, ind) = w.*vx1(:, ind) + 2 * r1 .* (pbestx1(:, ind) - presentx1(:, ind)) + 2 * r1 .* (pbestx1(gbest1, ind) - presentx1(:, ind));
            vx2 = vx1(:, ind);
            vx2(vx2 > upper_bound) = upper_bound;
            vx2(vx2 < lower_bound) = lower_bound;
            vx1(:, ind) = vx2;
            presentx1(:, ind) = presentx1(:, ind) + vx2;
            presentx2 = presentx1(:, ind);
            presentx2(presentx2 > upper_bound) = upper_bound;
            presentx2(presentx2 < lower_bound) = lower_bound;
            presentx1(:, ind) = presentx2;

            presentx3 = repmat(global_best_x1, num_initial1, 1);
            presentx3(:, ind) = presentx2;
            presenty1 = Rastrigin(presentx3);

            % 更新每个单独个体最佳位置
            pbestx2 = pbestx1(:, ind);
            pbestx2(presenty1 < pbesty1, :) = presentx2(presenty1 < pbesty1, :);
            pbestx1(:, ind) = pbestx2;
            pbesty1(presenty1 < pbesty1, :) = presenty1(presenty1 < pbesty1, :);
            
            % 更新所有个体最佳位置
            [fmin1, gbest1] = min(pbesty1);
            global_best_x1 = pbestx1(gbest1, :);
        end

        % 文中的s/2就是num_initial2,下面就是PSO算法
        ki = randi(num_initial2);
        while ki == gbest2
            ki = randi(num_initial2);
        end
        % 信息交互
        presentxx(ki, :) = global_best_x1;
        r2 = rand(num_initial2, num_vari);
        % pso更新下一步的位置,这里可以设置一下超过搜索范围的就设置为边界
        vxx = w.*vxx + 2 * r2 .* (pbestxx - presentxx) + 2 * r2 .* (pbestxx(gbest2, :) - presentxx);
        vxx(vxx > upper_bound) = upper_bound;
        vxx(vxx < lower_bound) = lower_bound;
        presentxx = presentxx + vxx;
        presentxx(presentxx > upper_bound) = upper_bound;
        presentxx(presentxx < lower_bound) = lower_bound;
        presentyy = Rastrigin(presentxx);
        % 更新每个单独个体最佳位置
        pbestxx(presentyy < pbestyy, :) = presentxx(presentyy < pbestyy, :);
        pbestyy(presentyy < pbestyy, :) = presentyy(presentyy < pbestyy, :);
        % 更新所有个体最佳位置
        [fmin2, gbest2] = min(pbestyy);
        % 信息交互
        for i2 = 1 : K
            kj = randi(num_initial1);
            while kj == gbest1
                kj = randi(num_initial1);
            end
            ind2 = ((1 + (i2 - 1) * sub_num) : i2 * sub_num);
            presentx1(kj, ind2) = pbestxx(gbest2, ind2);
        end

        %获取全局最优值
        if(fmin2 > fmin1)
            fmin = fmin1;
        else
            fmin = fmin2;
        end
        Fmin(n1, 1) = fmin;
        n1 = n1 +1;
        %fprintf("iter %d fmin: %.4f\n", i, fmin);
    end
    aver_Fmin = aver_Fmin + Fmin;
end
aver_Fmin = aver_Fmin ./ 100;
plot(aver_Fmin);